#include "Motor.h"
#include <stdlib.h>

static void set_pwm(TIM_HandleTypeDef * htim,uint32_t channel,uint16_t pin_a,uint16_t pin_b,int16_t duty){

	if(duty>0){
		  HAL_GPIO_WritePin(GPIOB, pin_a,GPIO_PIN_SET);
		  HAL_GPIO_WritePin(GPIOB, pin_b,GPIO_PIN_RESET);
	}else{

		  HAL_GPIO_WritePin(GPIOB, pin_a,GPIO_PIN_RESET);
		  HAL_GPIO_WritePin(GPIOB, pin_b,GPIO_PIN_SET);
	}

	uint16_t u_duty = abs(duty);

	if(u_duty>100){

		u_duty=100;
	}

  // 直接设置比较寄存器值（CCRx）
  __HAL_TIM_SET_COMPARE(htim, channel, u_duty);

}

void set_pwm_left(TIM_HandleTypeDef * htim,int16_t duty){
	set_pwm(htim,TIM_CHANNEL_1,GPIO_PIN_12,GPIO_PIN_13,duty);
}

void set_pwm_right(TIM_HandleTypeDef * htim,int16_t duty){
	set_pwm(htim,TIM_CHANNEL_2,GPIO_PIN_15,GPIO_PIN_14,duty);
}

/**
    读取编码器的位置增量（已处理编码器计数器溢出）
*/
int16_t read_encoder_delta_location(int16_t * prev,uint16_t raw_encoder_count) {


    int32_t delta = raw_encoder_count - *prev;
    if (delta > 32768) delta -= 65536; // 正向溢出修正
    else if (delta < -32768) delta += 65536; // 反向溢出修正
    *prev = raw_encoder_count;
    return delta;
}
